/*
 * Quad4.cpp
 *
 *  Created on: 16.01.2014
 *      Author: Stefan
 */

#include "Quad4.h"
#include "Property.h"
#include "Node.h"
#include "Material.h"
#include <iostream>
#include <math.h>

using namespace std;

void Quad4::createStressStrainMatrix() {
	if (iType == 2) {
		double dNue = pProperty->mMaterial->dPoissonsRatio;
		double dBeforeBracket = pProperty->mMaterial->dYoungsModulus / (1 - dNue * dNue);

		aStressStrainMatrix[0][0] = dBeforeBracket;
		aStressStrainMatrix[0][1] = dBeforeBracket * dNue;
		aStressStrainMatrix[1][1] = dBeforeBracket;
		aStressStrainMatrix[2][2] = dBeforeBracket * ((1 - dNue) / 2);
		aStressStrainMatrix[0][2] = 0;
		aStressStrainMatrix[1][2] = 0;
		aStressStrainMatrix[1][0] = aStressStrainMatrix[0][1];
		aStressStrainMatrix[2][0] = 0;
		aStressStrainMatrix[2][1] = 0;

		/*///// DEBUG
		cout << "Stress Strain Matrix:" << endl;
		for (int i = 0; i < 3; i++) {
			for (int j = 0; j < 3; j++) {
				cout << aStressStrainMatrix[i][j] << "\t";
			}
			cout << endl;
		}

		cout << endl;*/
	}
}

void Quad4::calculateStrainDisplacementMatrix(double dR, double dS) {

	//double aShapeFunction[4];
	double aShapeFunctionDerivatives[2][4];
	double aJacobiMatrix[2][2];
	double aInverseJacobiMatrix[2][2];

	//aShapeFunction[0] = .25 * (1 + dR) * (1 + dS);
	//aShapeFunction[1] = .25 * (1 - dR) * (1 + dS);
	//aShapeFunction[2] = .25 * (1 - dR) * (1 - dS);
	//aShapeFunction[3] = .25 * (1 + dR) * (1 - dS);

	//Derivatives of shape functions
	//Derive by r
	aShapeFunctionDerivatives[0][0] = .25 * (1 + dS);
	aShapeFunctionDerivatives[0][1] = -1 * aShapeFunctionDerivatives[0][0];
	aShapeFunctionDerivatives[0][2] = -.25 * (1 - dS);
	aShapeFunctionDerivatives[0][3] = -1 * aShapeFunctionDerivatives[0][2];
	//Derive by s
	aShapeFunctionDerivatives[1][0] = .25 * (1 + dR);
	aShapeFunctionDerivatives[1][1] = .25 * (1 - dR);
	aShapeFunctionDerivatives[1][2] = -1 * aShapeFunctionDerivatives[1][1];
	aShapeFunctionDerivatives[1][3] = -1 * aShapeFunctionDerivatives[1][0];

	//Jacobi matrix at point r|s
	for (int i = 0; i < 2; i++) {
		for (int j = 0; j < 2; j++) {
			double dTemp = 0;
			for (int k = 0; k < 4; k++) {
				dTemp += aShapeFunctionDerivatives[i][k] * aNodes[j][k];
			}
			aJacobiMatrix[i][j] = dTemp;
		}
	}

	//Calculate Jacobian
	dJacobian = aJacobiMatrix[0][0] * aJacobiMatrix[1][1] - aJacobiMatrix[0][1] * aJacobiMatrix[1][0];
	if (dJacobian < 0.00000001) {
		cout << "ERROR! Jacobian of element " << iId << " equal or smaller 0!" << endl;
	}

	//Calculate inverse of Jacobi matrix
	aInverseJacobiMatrix[0][0] = 1 / dJacobian * aJacobiMatrix[1][1];
	aInverseJacobiMatrix[1][0] = -1 / dJacobian * aJacobiMatrix[1][0];
	aInverseJacobiMatrix[0][1] = -1 / dJacobian * aJacobiMatrix[0][1];
	aInverseJacobiMatrix[1][1] = 1 / dJacobian * aJacobiMatrix[0][0];

	//Calculate Strain Dispacement matrix
	int k2 = -1;
	for (int k = 0; k < 4; k++) {
		k2 += 2;
		aStrainDisplacementMatrix[0][k2 - 1] = 0;
		aStrainDisplacementMatrix[0][k2] = 0;
		aStrainDisplacementMatrix[1][k2 - 1] = 0;
		aStrainDisplacementMatrix[1][k2] = 0;
		for (int i = 0; i < 2; i++) {
			aStrainDisplacementMatrix[0][k2 - 1] += aInverseJacobiMatrix[0][i] * aShapeFunctionDerivatives[i][k];
			aStrainDisplacementMatrix[1][k2] += aInverseJacobiMatrix[1][i] * aShapeFunctionDerivatives[i][k];
		}
		aStrainDisplacementMatrix[2][k2] = aStrainDisplacementMatrix[0][k2 - 1];
		aStrainDisplacementMatrix[2][k2 - 1] = aStrainDisplacementMatrix[1][k2];
	}

	/*//DEBUG
	cout << "Jacobi matrix" << endl;
	for (int i = 0; i < 2; i++) {
		for (int j = 0; j < 2; j++) {
			cout << aJacobiMatrix[i][j] << "\t";
		}
		cout << endl;
	}*/
}

void Quad4::createStiffnessMatrix() {
	//Clear array
	for (int i = 0; i < 8; i++) {
		for (int j = 0; j < 8; j++) {
			aStiffnessMartix[i][j] = 0;
		}
	}

//		if (iType > 1)
//			dThickness = 1;

	for (int iLx = 0; iLx < iIntegrationOrder; iLx++) {
		double dRIntegration = aGaussPointPosition[iIntegrationOrder - 1][iLx]; //R-coordinate of Gauss integration point
		for (int iLy = 0; iLy < iIntegrationOrder; iLy++) {
			double dSIntegration = aGaussPointPosition[iIntegrationOrder - 1][iLy]; //S-coordinate of Gauss integration point

			calculateStrainDisplacementMatrix(dRIntegration, dSIntegration);

			//DEBUG
			//cout << "Jacobian: " << dJacobian << endl;

			double dWeightingFactor = aGaussWeightingFactors[iIntegrationOrder - 1][iLx] * aGaussWeightingFactors[iIntegrationOrder - 1][iLy] * dThickness * dJacobian;
			for (int j = 0; j < 8; j++) {
				double aDB[3];
				for (int k = 0; k < 3; k++) {
					aDB[k] = 0;
					for (int l = 0; l < 3; l++)
						aDB[k] += aStressStrainMatrix[k][l] * aStrainDisplacementMatrix[l][j];
				}

				for (int i = j; i < 8; i++) {
					double dStiffness = 0;

					for (int l = 0; l < 3; l++)
						dStiffness += aStrainDisplacementMatrix[l][i] * aDB[l];

					//Add element to stiffness matrix
					aStiffnessMartix[i][j] += dStiffness * dWeightingFactor;
				}
			}
		}
	}

	//Make stiffness matrix full
	for (int j = 0; j < 8; j++)
		for (int i = j; i < 8; i++)
			aStiffnessMartix[j][i] = aStiffnessMartix[i][j];
}

void Quad4::calculateStrains() {
	calculateStrainDisplacementMatrix(0, 0); //Calculate B-Matrix at the element center
	//e=B*u
	for (int i = 0; i < 3; i++) {
		aStrains[i] = 0;
		for (int j = 0; j < 8; j++) {
			aStrains[i] += aDisplacements[j] * aStrainDisplacementMatrix[i][j];
			//cout << aDisplacements[j] << "*" << aStrainDisplacementMatrix[i][j] << " + ";
		}
		//cout << endl;
	}
}

void Quad4::calculateStresses() {
	//s=C*e
	for (int i = 0; i < 3; i++) {
		aStresses[i] = 0;
		for (int j = 0; j < 3; j++) {
			aStresses[i]+=aStressStrainMatrix[i][j]*aStrains[j];
		}
	}
}

void Quad4::getDisplacements(){
	aDisplacements[0] = nNodes[0]->fDisplacements[0];
	aDisplacements[1] = nNodes[0]->fDisplacements[1];
	aDisplacements[2] = nNodes[1]->fDisplacements[0];
	aDisplacements[3] = nNodes[1]->fDisplacements[1];
	aDisplacements[4] = nNodes[2]->fDisplacements[0];
	aDisplacements[5] = nNodes[2]->fDisplacements[1];
	aDisplacements[6] = nNodes[3]->fDisplacements[0];
	aDisplacements[7] = nNodes[3]->fDisplacements[1];
	//cout << aDisplacements[7];
}

void Quad4::calculateResults() {
	getDisplacements();
	calculateStrains();
	calculateStresses();
}

Quad4::Quad4(int iId, Property *pProperty, Node *nN1, Node *nN2, Node *nN3, Node *nN4) {
	this->iId = iId;
	this->pProperty = pProperty;
	aNodes[0][0] = nN1->iX;
	aNodes[1][0] = nN1->iY;
	aNodes[0][1] = nN2->iX;
	aNodes[1][1] = nN2->iY;
	aNodes[0][2] = nN3->iX;
	aNodes[1][2] = nN3->iY;
	aNodes[0][3] = nN4->iX;
	aNodes[1][3] = nN4->iY;

	nNodes[0] = nN1;
	nNodes[1] = nN2;
	nNodes[2] = nN3;
	nNodes[3] = nN4;

//	aNodeNumbers[0] = nN1->iId;
//	aNodeNumbers[1] = nN2->iId;
//	aNodeNumbers[2] = nN3->iId;
//	aNodeNumbers[3] = nN4->iId;

	//Set DOFs active
	nN1->aDOFs[0] = true;
	nN1->aDOFs[1] = true;
	nN2->aDOFs[0] = true;
	nN2->aDOFs[1] = true;
	nN3->aDOFs[0] = true;
	nN3->aDOFs[1] = true;
	nN4->aDOFs[0] = true;
	nN4->aDOFs[1] = true;

	iIntegrationOrder = 2;

	dThickness = pProperty->dReal1;
	iType = pProperty->iProp1;

	//cout << "QUAD4 Element initialized" << endl;

	//cout << "Creating QUAD4 Element with nodes: " << nN1->iId << " " << nN2->iId << " " << nN3->iId << " " << nN4->iId << endl;

	createStressStrainMatrix();

	createStiffnessMatrix();

	/*cout << "Stiffness matrix created!" << endl;
	///DEBUG
	for (int i = 0; i < 8; i++) {
		for (int j = 0; j < 8; j++) {
			cout << aStiffnessMartix[i][j] << "\t";
		}
		cout << endl;
	}*/
}

